//
// Created by jg on 2021/1/2.
//

#ifndef PLAN_ENV_PLAN_MAP_H
#define PLAN_ENV_PLAN_MAP_H

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <mutex>
#include <memory>

#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>

#include <pos_generator/loc2pos.hpp>

#include "env_edt/utils.h"
#include "env_edt/corridor_generator.h"
#include "sdf_tools/collision_map.hpp"

#define MAP_PATH "/home/jg/catkin_ws/btraj/wmr_bezier_planner/map/gazebo_willowgarage_index.map";

using std::shared_ptr;
using std::vector;
using std::pair;

enum PLANSTYLE {GLOBAL = 0, LOCAL};

class MAP_EDT{
public:
    typedef shared_ptr<sdf_tools::CollisionMapGrid> Map_Ptr;
    typedef sdf_tools::CollisionMapGrid::DistanceField DSF;
protected:
    ros::NodeHandle n_;

    double resolution_, inv_resolution_, map_margin_;
    double inflate_size_, MAX_VEL_;
    double x_local_size_, y_local_size_;
    double x_min_, x_max_, y_min_, y_max_;
    bool   has_map_, is_mapping_, has_local_map_;
    std::string file_name_;
    std::ifstream fp_;
    std::ofstream wf_;

    std::mutex    file_lock_, map_lock_;
    vector<pair<Eigen::Vector3d, bool >> vec_pt_;

    sdf_tools::COLLISION_CELL free_cell_;
    sdf_tools::COLLISION_CELL obst_cell_;

    Map_Ptr collision_map_;
    Map_Ptr inflate_map_;
    Map_Ptr collision_map_local_;

    Eigen::Vector3d map_origin_, local_origin_, robot_pt_, origin_BLH_;

    const float lofree = -5, looccu = 100;
    const float oddsmax = 128, oddsmin = -127;

    DSF esdf_;
    CORRIDOR_GENERATOR::Ptr corridor_2D_;
public:
    MAP_EDT(): has_map_(false), has_local_map_(false) {
        file_name_    = MAP_PATH;
        x_local_size_ = -1;
        y_local_size_ = -1;
        map_margin_   = -1;
        MAX_VEL_      = -1;
        inflate_size_ = -1;
        is_mapping_   = -1;

        x_min_ = x_max_ = y_min_ = y_max_ = 0;

        resolution_     = -1;
        inv_resolution_ = -1;
    }

    MAP_EDT(std::string _file,  double x_lsize, double y_lsize, double resolution,
                  bool is_map = false ,double margin = 0, double maxv = 1, double inflate = 0.3)
            : has_map_(false), has_local_map_(false) {
        file_name_    = std::move(_file);
        x_local_size_ = x_lsize;
        y_local_size_ = y_lsize;
        map_margin_   = margin;
        MAX_VEL_      = maxv;
        inflate_size_ = inflate;
        is_mapping_   = is_map;

        x_min_ = x_max_ = y_min_ = y_max_ = 0;

        resolution_     = resolution;
        inv_resolution_ = 1.0 / resolution_;

        collision_map_       = std::make_shared<sdf_tools::CollisionMapGrid>();
        inflate_map_         = std::make_shared<sdf_tools::CollisionMapGrid>();
        collision_map_local_ = std::make_shared<sdf_tools::CollisionMapGrid>();

//        loadFile();
    }

    ~MAP_EDT(){
        //write collision_map_'s data to file
        if(is_mapping_) writeFile();

        fp_.close();
        wf_.close();

        vec_pt_.clear();
        collision_map_       = nullptr;
        collision_map_local_ = nullptr;
        inflate_map_         = nullptr;
    }

    void init(ros::NodeHandle& nh);
    void builtLocalMap(const Eigen::Vector3d& start_pt);
    void refreshMap(double _x, double _y, vector<pair<Eigen::Vector3d, bool >>& _vec_point);
    bool checkCoordObs(Eigen::Vector3d checkPt);

    double getResolution() {return resolution_;}
    double getLocalMapSize() {return x_local_size_;}
    bool getGlobalMap(Map_Ptr& _map);
    bool getLocalMap(Map_Ptr& _map);
    void getMapOrigin(Eigen::Vector3d& _origin);
    void getLocalOrigin(Eigen::Vector3d& _origin);
    vector<int64_t> location2Index(const Eigen::Vector3d& location, PLANSTYLE style);
    void GlobalIndex2Location(const vector<int64_t>& index, Eigen::Vector3d& pt);
    void getMapSize(vector<double>& map_size);
    void getOrigBLH(Eigen::Vector3d& original);

    bool checkPOS(const Eigen::Vector3d& _pt){
        //-true : robot's position inside the global map
        //-false : not inside
        const double& rob_x = _pt[0];
        const double& rob_y = _pt[1];
        return !(rob_x < x_min_ || rob_x > x_max_ || rob_y < y_min_ || rob_y > y_max_);
    }

    void getFMMGrid(FMGrid3D& grid_fmm, double& max_vel);
    void FMMPath3D2vecPath(Path3D& path3D, vector<Eigen::Vector3d>& path_coord);
    pair<vector<unsigned int>, unsigned int> FMMSet2Point
    (FMGrid3D& grid_fmm, Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);
    void getCorridor(vector<Cube>& corridor, vector<Eigen::Vector3d>& path, vector<double>& time);

    void getDistance(double &dis, Eigen::Vector3d& pt);
    void getDistance(double &dis, vector<int64_t>& pt);
    void evaluateEDTWithGrad(const Eigen::Vector3d &pos, double &dist, Eigen::Vector3d &grad);

    void setGlobalMapMarker(visualization_msgs::Marker& map_marker);
    void setLocalMapMarker(visualization_msgs::Marker& map_marker);
    void visMapBoundery(visualization_msgs::Marker& map_marker);

    typedef shared_ptr<MAP_EDT> Ptr;
protected:
    void pointInflate();
    void loadFile();
    void writeFile();
    void builtGlobalMap();
    void reMappingGMap(Eigen::Vector3d pt);
    void bresenHamLine(shared_ptr<sdf_tools::CollisionMapGrid>& _map ,int64_t x0, int64_t y0, int64_t& x1, int64_t& y1);

    void setGMapFromLocal(Eigen::Vector3i& index_local, sdf_tools::COLLISION_CELL odds) {
        auto loc = collision_map_local_->GridIndexToLocation(index_local);
        if(is_mapping_)
            collision_map_->Set3d(loc, odds);

        inflate_map_->Set3d(loc, odds);
    }

    vector<int64_t> getGroundIndex(bool mark ,Eigen::Vector3d& pt, const double& _x, const double& _y);

    double velMapping(const Eigen::Vector3d& pt, double d, double max_v);

    void getSurroundPts(const Eigen::Vector3d& pos, Eigen::Vector3d pts[2][2][2], Eigen::Vector3d &diff);
    void getSurroundDistance(Eigen::Vector3d pts[2][2][2], double dists[2][2][2]);
    void interpolateTrilinear(double values[2][2][2], const Eigen::Vector3d& diff, double& value,
                              Eigen::Vector3d &grad);
};

#endif //PLAN_ENV_PLAN_MAP_H
